import { ArrowHelper, Matrix4, Quaternion, Vector3 } from "three";





export class Joint {



    boneInitDirection = []

    boneNode = []


    constructor(root, nameMap, initQuaternion = new Quaternion()) {

        this.root = root
        this.nameMap = nameMap
        this.initQuaternion = initQuaternion


        this.boneNode = nameMap.map(([boneName]) => {
            return root.getObjectByName(boneName);
        })

        console.log(this.boneNode)



    }


    update(states) {



        this.boneNode.forEach(v => {
            v.quaternion.identity()
        })




        const worldPositions = this.nameMap.map(([, name]) => states[name].position)





        const mat_a = new Matrix4()
        {
            mat_a.makeTranslation(worldPositions[0].x, worldPositions[0].y, worldPositions[0].z)
        }

        const mat_1 = new Matrix4()
        {

            const v3 = new Vector3();
            this.boneNode[0].getWorldPosition(v3)
            mat_1.makeTranslation(v3.x, v3.y, v3.z)
        }




        const lastQu = new Quaternion()
        for (let i = 1; i < this.nameMap.length; i++) {
            const qu = new Quaternion();

            const end = new Vector3()
            const from = new Vector3()


            const qu2 = new Quaternion()



            {

                const invert = mat_a.clone().invert()
                const v3 = worldPositions[i].clone().applyMatrix4(invert)



                end.copy(v3).normalize()




                const out = new Matrix4()

                const base = new Vector3()
                base.setFromMatrixColumn(mat_a, 2).normalize()

                // const qu = new Quaternion()
                // qu.setFromUnitVectors(base, end)

                // end.applyQuaternion(qu)


                qu2.setFromUnitVectors(new Vector3(0, 0, 1), end)



                out.makeRotationFromQuaternion(qu2)

                const translate = new Matrix4()
                translate.makeTranslation(0, 0, v3.length())
                out.multiply(translate)


                mat_a.multiply(out)





                // { // TODO 带删除

                //     const qu = new Quaternion()
                //     qu.setFromRotationMatrix(mat_a)

                //     // const cone = new Mesh(new ConeGeometry(10, 10, 10), new MeshBasicMaterial({ depthTest: false, color: Math.random() * 0xffffff | 0 }))

                //     cone.matrixAutoUpdate = false

                //     const mat_c = mat_a.clone()

                //     const axes = new AxesHelper(20)
                //     axes.matrixAutoUpdate = false
                //     axes.matrix.copy(mat_c)
                //     // this.root.add(axes)

                //     // cone.matrix.copy(mat_c)


                //     // this.root.add(cone)
                // }




            }



            {
                const invert = mat_1.clone().invert()

                const v3 = new Vector3()

                this.boneNode[i].getWorldPosition(v3)

                v3.applyMatrix4(invert)



                from.copy(v3).normalize()



                const length = v3.length()

                const out = new Matrix4()



                out.makeRotationFromQuaternion(qu2)

                const translate = new Matrix4()
                translate.makeTranslation(0, 0, length)
                out.multiply(translate)


                mat_1.multiply(out)



            }

            if (i > 1) {




                const a = new Quaternion().setFromRotationMatrix(this.boneNode[i - 2].matrix)
                const b = lastQu.clone()




                const qx = a.clone().invert().multiply(b);



                from.applyQuaternion(qx)
                end.applyQuaternion(qx)

            }


            qu.setFromUnitVectors(from, end)


            this.boneNode[i - 1].quaternion.copy(qu)

            lastQu.copy(qu2)




        }







        return


        this.nameMap.slice(0, -1).forEach((_, index) => {
            const qu = new Quaternion();
            if (index >= 1) {
                const v3 = this.boneInitDirection[index - 1].clone();
                qu.setFromUnitVectors(new Vector3(1, 0, 0), v3.clone().normalize());

                const v32 = this.boneInitDirection[index].clone();

                v32.applyQuaternion(qu.clone());

                const qu2 = new Quaternion();

                // qu2.setFromUnitVectors(v32, new Vector3(0.995, 0.086, 0.036));
                qu2.setFromUnitVectors(v32, localVectors[index]);

                qu.multiply(qu2.clone());
            } else {


                qu.setFromUnitVectors(this.boneInitDirection[index].clone(), localVectors[index]);

                qu.premultiply(this.initQuaternion.clone());
                // qu.multiply(this.initQuaternion.clone().invert());
                // qu = this.initQuaternion.clone().multiply(qu);
            }

            const target = this.root.getObjectByName(this.nameMap[index][0]);

            if (target) {
                target.quaternion.copy(qu);

                // target.weight

            }

        })



    }




    computeBoneDirection(worldPositions) {
        const localVectors = []




        const matrix = new Matrix4()
        matrix.makeTranslation(worldPositions[0].x, worldPositions[0].y, worldPositions[0].z)
        const mt4 = new Matrix4()
        const mt4_2 = new Matrix4()
        const quad = new Quaternion()

        // 处理每个子节点
        for (let i = 1; i < worldPositions.length; i++) {

            const parentPos = worldPositions[i - 1].clone()
            const nodeWorldPos = worldPositions[i].clone();

            const length = nodeWorldPos.distanceTo(parentPos)


            const v3 = nodeWorldPos.applyMatrix4(matrix.clone().invert());

            const v3Normal = v3.clone().normalize()

            localVectors.push(v3Normal)



            const v32 = new Vector3(0, 0, 1).multiplyScalar(length)

            mt4.makeTranslation(v32)


            if (parentPos) {

                const base = new Vector3(0, 0, 1)
                base.setFromMatrixColumn(matrix, 2).normalize();

                quad.setFromUnitVectors(base, v3Normal)

                mt4_2.makeRotationFromQuaternion(quad)

                mt4.multiply(mt4_2)
            }

            matrix.multiply(mt4)
        }

        return localVectors



    }
}